#pragma once
#include "ros/ros.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Twist.h"
#include "common_lib/common.h"
#include "tf/tf.h"

class ModelBase 
{
public:
	ModelBase(float x, float y, float theta) : x_(x), y_(y), theta_(theta) { Init(); };
	void Init()
	{
		is_charge_ = false;
		work_state_ = 0;
	};
	geometry_msgs::Pose GetPose()
	{
		geometry_msgs::Pose pose;
		pose.position.x = x_;
		pose.position.y = y_;
		pose.orientation = tf::createQuaternionMsgFromYaw(theta_);;
		return pose;
	};
	std::vector<float> GetTwist()
	{
		std::vector<float> twist;
		twist.push_back(d_theta_);
		twist.push_back(v_);
		return twist;
	}
	float GetWorkState()
	{
		return work_state_;
	}
	bool GetChargeState()
	{
		return is_charge_;
	};
	void SetPose(float x, float y, float theta)
	{
		x_ = x;
		y_ = y;
		theta_ = theta;
	};
	virtual void MoveModelProcess(float speed, float angle_speed,float speed_toward) = 0;
	virtual void WorkModelProcess(float target_height, float height)
	{
		work_state_ = target_height;
	};
	virtual void WorkModelProcess(float target_speed)
	{
		
	};
	virtual void ChargeModelProcess(bool is_charge)
	{
		is_charge_ = is_charge;
	};

	float x_;
	float y_;
	float theta_;
	float speed_dir_;	
	float v_;
	float d_theta_;
	float work_state_;// height or speed
	bool is_charge_;
	common_lib::Common common_cal_;
};